using System;
using System.Collections.Generic;
using System.Text;
using SyncedFileAccess;
using System.Threading;
//using System.Drawing;

namespace RobotCtrl
{
	public class Robot
	{
        private RobotConsole console;
        private Drive drive;
        private Radar radar;
        private DrivePosition relRadarPosition;
        private RunMode runMode;

		public Drive Drive { get { return drive; } }
		public RobotConsole Console { get { return console; } }
		public DrivePosition Position{get{return drive.Position;}set{drive.Position = value;}}
        public Radar Radar { get { return radar; } set { radar = value; } }
        public DrivePosition RelativeRadarPosition { get { return relRadarPosition; } set { relRadarPosition = value; } }
        public RunMode RunMode { get { return runMode; } }

        //Teil2-Dir
        private List<Track> path; //Path built via gui 
        private SyncedFile drivenPath;

		public Robot(RunMode runMode)
		{
            if (!Config.IsWinCE)
                runMode = RunMode.VIRTUAL;
            this.runMode = runMode;
			console = new RobotConsole(runMode);
			drive = new Drive(this, runMode);
            radar = new Radar(this, runMode);
            path = new List<Track>();
		}

        public Robot(RunMode runMode, SyncedFile drivenPath)
            : this(runMode)
        {
            this.drivenPath = drivenPath;
        }

		public void Close()
		{
			drive.Close();
		}

        //Teil2-Dir
        /// <summary>
        /// Runs a specifi program (track) consisting of multiple path parts
        /// </summary>
        public void runProgramInPath()
        {
            this.drive.Power = true;

            //drive Path that has been built with trackbuilder
            {
                foreach (Track t in path)
                {
                    if (t is TrackArcLeft)
                    {
                        TrackArcLeft tAL = (TrackArcLeft)t;
                        this.drive.RunArcLeft(tAL.radius, tAL.angle, tAL.speed, tAL.acceleration);
                        drivenPath.WriteLine("TrackLeft \t Radius: " + tAL.radius + " Angle: " + tAL.angle + " Speed: " + tAL.speed
                            + " Accel: " + tAL.acceleration);
                    }
                    if (t is TrackArcRight)
                    {
                        TrackArcRight tAR = (TrackArcRight)t;
                        this.drive.RunArcRight(tAR.radius, tAR.angle, tAR.speed, tAR.acceleration);
                        drivenPath.WriteLine("TrackRight \t Radius: " + tAR.radius + " Angle: " + tAR.angle + " Speed: " + tAR.speed
                            + " Accel: " + tAR.acceleration);
                    }
                    if (t is TrackLine)
                    {
                        TrackLine tL = (TrackLine)t;
                        this.drive.RunLine(tL.length, tL.speed, tL.acceleration);
                        drivenPath.WriteLine("TrackLine \t Length: " + tL.length + " Speed: " + tL.speed + " Accel: " + tL.acceleration);
                    }
                    //Hack
                    if (t is TrackRadar)
                    {
                        this.drive.RunLine(t.length, t.speed, t.acceleration);
                    }

                    if (t is TrackPause)
                    {
                        TrackPause tP = (TrackPause)t;
                        this.drive.RunPause(tP.pause);
                    }
                    if (t is TrackTurn)
                    {
                        TrackTurn tT = (TrackTurn)t;
                        this.drive.RunTurn(tT.angle, tT.speed, tT.acceleration);
                        drivenPath.WriteLine("TrackTurn \t Angle: " + tT.angle + " Speed: " + tT.speed
                            + " Accel: " + tT.acceleration);
                    }
                    //TODO
                    //Wait for robot to finish this part of the path
                    while (!this.drive.Done)
                    {
                        Thread.Sleep(5); //give some time to other tasks
                    }
                }
                path.Clear();
            }

            this.drive.Power = false;
            //this.isRunning = false;
        }

        public void addTrack(Track t)
        {
            path.Add(t);
        }

	}
}
